/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * @file CAHistogramECorrelationScanMatch.cpp
 * 
 * This is a laser scan matching algorithm, i.e. it calculates
 * translation (x,y) and rotation (angle) between two laser scans.
 * 
 * The algorithm is divided into two quite separate parts that 
 * are combined to produce the final match (x, y, angle):
 * Part 1) Angle correction algorithm: based on laser scans' angle 
 *         histograms.
 * Part 2) Translation correction algorithm: based on iterative 
 *         evidence grid correlation.
 *  
 **************************************************************
 * Part 1: Angle Histogram Correlation with Sequential Lines
 *
 * The algorithm corrects the angle between two laser scans by using
 * the laser scans' angle histograms, i.e. using a representation of
 * the frequency distribution of data values. If the consecutive scans
 * represent the same environment, the histogram of the reference scan
 * and the latest measurement are the same. If the scans are taken from
 * dierent angles, then the histogram in the other scan is only
 * shifted. The histogram calculation relies on the fact that the
 * points received from laser range nder are ordered, i.e. one distance
 * measurement after every 0.5 degrees is received.
 *
 * The angle histogram is calculated by computing the angle between
 * adjacent scan points. The original idea of the angle histogram is to
 * calculate the angle between two consecutive scan points. This
 * however does not provide accurate angle histogram, due to the sensor
 * uncertainty. The solution is to calculate the histogram for points
 * separated by some xed number of consecutive points. The number of
 * points used for angle calculations is an adjustable parameter of the
 * algorithm.
 *
 * The algorithm steps are:
 *
 * 1) Calculate angles from the laser scan for the angle histogram 
 * using some xed spacing between scan points, e.g. with step size 15.
 * 
 * 2) Discretise the calculated angles and calculate the
 * angle frequency count, i.e. the angle histogram.
 * 
 * 3) Calculate cross-correlation for the angle histograms over the
 * wanted search area, e.g. 45 degrees search area with 0.5 degree step
 * means that and thus has to be calculated in 45*2*2+1=181 points. 
 * 
 * 4) Save all local maximum cross-correlations and the corresponding
 * angles.
 *  
 * 5) Sort these crosscorrelations and return wanted number of best 
 * correlating angles.
 * 
 * The algorithm gives good estimates especially when there are long
 * lines in the scans, e.g. walls. The step size has to be large enough
 * to smooth the measurement inaccuracies but small enough to be able to
 * see some details.  A step size around 30 was found to be the most
 * reliable when using laser scans with 361 points.
 *
 **************************************************************
 * Part 2: Location Correction with Evidence Grid Correlation 
 *
 * Location correction with evidence grid correlation returns the best
 * correlating shift in x- and y-directions for two scans. Correction
 * is done only for the translation and it is assumed that the angle
 * deviation between scans is already corrected.
 *
 * The algorithm steps are:
 * 
 * 1) Scan coordinates are discretised in the search area (between some
 * minimum and maximum coordinates), e.g. x=[-10, 10], y=[0, 15].
 * 
 * 2) Coordinates are sorted according to y-values using quicksort
 * algorithm to speed up the search. The values in the y-direction are
 * usually more spread and thus finding the right y-value narrows the
 * distance search more than if searching in x-direction. This is
 * because walls seen by the laser scanner are usually parallel to
 * direction of forward movement, i.e.  in the direction of the y-axis.
 *
 * 3) Select the x and y search area, and the search step size. For
 * example search area of 20cm times 20cm with 1cm step size could be
 * selected.
 *
 * 4) For every discrete point from the first scan, search the nearest
 * discrete point from the second scan. This requires a lot of
 * computation power, although the quicksorting was done to ease this
 * task.  The quicksort limits the correlation algorithm search area
 * into a smaller portion of the array, i.e. if we know that y-values
 * are between 1.2 and 1.6 then we can jump directly to the right part
 * of the sorted array. An index is used to keep a track with the
 * search area in the sorted array, so that the next nearest point
 * search can continue from where the last one ended. In the
 * worst-case, all the y-values are within the search area which means
 * that all the 361 scan points have to be compared with all 361 points
 * of the other scan. In the best case the values are distributed so
 * that search area is only one point, e.g. when the scans are almost
 * identical.
 * 
 * 5) The nearest point is estimated to be hit (0) or miss (1), or
 * something between those two, e.g.  0.45 is a partial hit. 
 * 
 * 6) Sum all the hit values for each scan to calculate correlation
 * value for the matched scans. Small correlation value indicates good
 * match, i.e.  largest number of hits found.
 *
 * 7) Return wanted number of best matching angles with associated
 * correlation value. The returned correlation values can be used to
 * estimate the goodness of the match.
 *
 * The matching algorithm has diculties when there are no details
 * against which to match. For example a straight corridor with no
 * objects and with an upward stairs at the end is very dicult.  The
 * best results are gained when translations between several scans are
 * calculated to verify the match,
 *
 * Because the complexity of this algorithm is O(N^2) it is not
 * possible to use very large search areas in real-time.  This problem
 * can be partially solved by using iterative calculations. This means
 * that we first calculate a solution with large step size and search
 * area, which is then used to limit the next search area with smaller
 * step size. NOTE! This iterative approach is used in this
 * implementation.
 *
 **************************************************************
 * TODO:
 * - Fix all (3) the issues marked with FIXME tag. 
 * - Making the code more C++ than C style.
 * - Consider removing commented code (mostly debugging prints)
 *
 **************************************************************
 *
 * @author Seppo Heikkil� * @acknoledgements Jari Saarinen
 * 
 * References:
 * [1] Heikkil�S., Development of Laser Based Localisation Methods for 
 *     Personal Navigation System - PeNa, Master's thesis, Helsinki
 *     University of Technology, March, 2005, Espoo, Finland.
 *
 */

#include <time.h>
#include "CAHistogramECorrelationScanMatch.h"

/**
 * Default constructor
 * Sets default parameters
 * Assumes: 361 measurement with 0.5 deg... 
 */
CAHistogramECorrelationScanMatch::CAHistogramECorrelationScanMatch() 
{
  // The function of these variables are described in .h file
  // Default values are from the PeLoTe project code
  angle_resolution = 0.01;
  pi = 3.1415926535897932384626433832795;
  //searcharea = 0.32/2.0/2.0; // For robot (in meters)
  searcharea = 0.32; // For human (in meters)
  anglesearchdist = 0.5;
}
	
/**
 * Initializes the laser scan matcher with custom search area and
 * search angle.
 *
 * @param searchArea size of xy-correlation algorithm search area
 * (meters). Has to be dividable by 0.04.
 * @param angleSearchArea distance for the angle to be searched (radians).
 */
CAHistogramECorrelationScanMatch::CAHistogramECorrelationScanMatch(float searchArea, float angleSearchArea){
  // The function of these variables are described in .h file
  // Default values are from the PeLoTe project code
  angle_resolution = 0.01;
  pi = 3.1415926535897932384626433832795;
  // FIXME: use default constructor to do above initializatins

  searcharea = searchArea;
  anglesearchdist = angleSearchArea;
}

CAHistogramECorrelationScanMatch::~CAHistogramECorrelationScanMatch(){
  
}
        
/////////////////////////////////////////////
//// SCAN MATCH INTERFACE ///////////////////
////////////////////////////////////////////
/**
 * Updates the position estimate with the new measurement @meas. 
 * @param &meas The latest range scan measurement
 * @param &est  The estimated position, where the measurement is from.
 * @return Goodness The goodness (probability/likelihood) of the match [0...1] 1 is perfect 0 is none 
 */
float CAHistogramECorrelationScanMatch::update(ScanMatch::scan &meas, ScanMatch::pose est){
  float result = 0;
  clock_t t1,t2;
  float t;
  int hit;

  static float refxo[361], refyo[361];
  static float actxo[361], actyo[361];
  int actxyN = 0, refxyN = 0;
  float pose_est[3] = {0.0, 0.0, 0.0};
  float pose_new[3] = {0.0, 0.0, 0.0};
  float correlation = 0;
  float S1[2] = {0,0}, S2[2] = {0,0};
  float *S[2] = {S1, S2};
  float anglevar = 0;
  float estf[3] = {0.0, 0.0, 0.0};
 
  //fprintf(stderr,"Estimating... pos now (%.2f,%.2f,%.2f)", pos.x,pos.y,pos.a);
  t1=clock();

  // converter data to right format
  //refxyN = distance2xycoordinates(ref.r, refxo, refyo); // distances => (x, y)-coordinates
  //actxyN = distance2xycoordinates(meas.r, actxo, actyo); // distances => (x, y)-coordinates
	refxyN = distance2xycoordinates(ref, refxo, refyo); // distances => (x, y)-coordinates
	actxyN = distance2xycoordinates(meas, actxo, actyo); // distances => (x, y)-coordinates
  
	//estf[0] = est.x;estf[1] = est.y;estf[2] = est.a;
	estf[0] = 0;estf[1] = 0;estf[2] = 0;
	
	ScanMatch::pose diff = est.calculateDifferentialSensorMovement(rpos,est,0);
	diff.topi();
	//fprintf(stderr,"diff:(%.2f %.2f %.2f) a(0)=%.2f \n",diff.x,diff.y,diff.a,meas.a[0]);
	
	pose_est[0] = diff.x;pose_est[1]=diff.y; pose_est[2] = diff.a;
  
	hit = matchScans(refxo, refyo, refxyN,
		   actxo, actyo, actxyN,
		   pose_est, pose_new,
		   &correlation, S, &anglevar,
		   searcharea, anglesearchdist);
  
	float dl = sqrt(pose_new[0]*pose_new[0] + pose_new[1]*pose_new[1]);
	float da = atan2(pose_new[1],pose_new[0]);
	float dx = dl * cos(meas.a[0]+da);
	float dy = dl * sin(meas.a[0]+da);
	
	//fprintf(stderr,"RES: (%.2f %.2f %.2f) ",pose_new[0],pose_new[1],pose_new[2]);
	pose_new[0] = dx; pose_new[1] = dy;
	
	estf[0]=pos.x;estf[1]=pos.y;estf[2]=pos.a;
	//integrate(estf, pose_new, pose_est);
	pos = pos.integrateDifferential(ScanMatch::pose(pose_new[0],pose_new[1],pose_new[2]));

  // Use correlation to tell the accuracy
  // FIXME: The covariance ellipse could be better to estimate the goodness
  if (correlation > 1000) result = 0;
  else result = (float)correlation/1000.0;

  t2=clock();
  t = (float)(t2-t1)/(float) CLOCKS_PER_SEC;
 //fprintf(stderr,"pos now (%.2f,%.2f,%.2f) time elapsed:%.2f corr=%.2f\n",pos.x,pos.y,pos.a,t, correlation);

  return result;
}
  
////////////////////////////////////////////////////////////////////////////////////
//// CAHistogramECorrelationScanMatch - CANNOT BE USED FROM SCANMATCH INTERFACE ////
////////////////////////////////////////////////////////////////////////////////////


/**
 * Match the two given scans. Returns how much first scan(act) has to be 
 * shifted for it to be aligned with last one(ref).
 * 
 * @param refxo Reference scan x-coordinates.
 * @param refyo Reference scan y-coordinates.
 * @param refxyN Reference scan array sizes.
 * @param actxo Reference scan x-coordinates.
 * @param actyo Reference scan y-coordinates.
 * @param actxyN Reference scan array sizes.
 * @param itialpose initial guess for the correction.
 * @param retCorrection returned correction between scans.
 * @param correlation xy correlation for the match.
 * @param S location covariace matrix.
 * @param anglevar angle variance is returned here.
 * @param searcharea size of xy-correlation algorithm search area.
 * @param anglesearchdist distance for the angle to be searched, in radians
 *
 * @return 0 if all ok, else -1
 */
int CAHistogramECorrelationScanMatch::matchScans(float *refxo, float *refyo, int refxyN,
					 float *actxo, float *actyo, int actxyN,
					 float *initialpose, float *retCorrection, 
					 float *correlation, float **S, float *anglevar,
					 float searcharea, float anglesearchdist) {
        // Angle estimation variables
        int number_of_maximums = 2;        // Number of gueses that will be processed
        float angle_buffer[2];             // Best angle guesses are fetched here
        float angle_buffer_correlation[2]; // Correlations for best angle guesses are fetched here
        static float refx[361], refy[361], // xy-coordinates of scans
                     actx[361], acty[361];                
        float retpose[3];                  // processed poses

        // Location match variables
        // Searcher defines search area, searcher2 defines stepsize
        float searcher[] =  {(float)1.28, (float)0.64, (float)0.32, (float)0.16, (float)0.08, (float)0.04};
        float searcher2[] = {(float)0.64, (float)0.32, (float)0.16, (float)0.08, (float)0.04, (float)0.02};
        float searcher_x_sum = 0;          // Initial guess for x searcher sum
        float searcher_y_sum = 0;          // Initial guess for y searcher sum
        int   max_searcher_size = 6;       // searchers size == size(searcher) == size(searcher2) 
        int   searcher_size     = 6;       // searchers size used
        float best_correlation = (float)666666666, // Best total correlation found, the smaller, the better
              match_correlation = 0;       // Total correlation of latest match
        float best_x = 0, best_y = 0,      // Pose with best found match
              best_angle = 0;   
        float bestxy_correlation = 0;      // Best correlation from single match 
        // Covariance matrix of the location match
        float ssdata[2][2];
        float *SS[2];
        //float rangle = 0;

        // Temporary, free-to-use variables
        clock_t t1 = clock(), t2;
        //int tmp = 0;  
        int i = 0, k = 0;
        float ftmp = 0;
        //float pose[3];
        //FILE *filetmp = fopen("anglecor.txt", "w");
        //FILE *filetmp2 = fopen("anglecorangs.txt", "w");

        // Check parameters
        if(refxo == NULL || refyo == NULL || refxyN <= 0 ||
           actxo == NULL || actyo == NULL || actxyN <= 0 ||
           initialpose == NULL || retCorrection == NULL ||
           correlation == NULL || S == NULL || anglevar == NULL ||
           searcharea <= 0 || anglesearchdist <= 0) {
                fprintf(stderr, "int CAHistogramECorrelationScanMatch::matchScans: invalid parameter(s).\n");
                return -1;
        }

        // Set covariance matrix
        SS[0] = ssdata[0];
        SS[1] = ssdata[1];

        // Define how many rounds with given searcharea
        ftmp = (float)searcher[max_searcher_size-1];
        searcher_size = 1;
        while(1) {
                ftmp *= 2;
                if(ftmp > searcharea)
                        break;
                searcher_size++;
                if(searcher_size > max_searcher_size) break;
        }
        if(searcher_size > max_searcher_size) searcher_size = max_searcher_size;
        if(searcher_size < 2) searcher_size = 2;
                // Copy values matching the searcher size
        for(i = 0; searcher_size > i; i++) {
                searcher[i]  = searcher[i+max_searcher_size-searcher_size];
                searcher2[i] = searcher2[i+max_searcher_size-searcher_size];
        }

        /////////////////////////////////////////////////////////////////////
        ////////////////////// Angle calculation start //////////////////////
        t1 = clock();
        
        // 0. area 0.1 and res 0.01, 5 steps => 1.8 ms angle search time
        // 1. area 0.1 and res 0.01, 5 steps => 1.4 ms angle search time
        number_of_maximums = estimateAngle(refxo, refyo, refxyN, 
                                                                   actxo, actyo, actxyN,
                anglesearchdist, (float)angle_resolution,  // search area = see above, resolution = see above (degrees)
                -initialpose[2], // initial "guess"
                angle_buffer, angle_buffer_correlation, number_of_maximums,
                anglevar);

        /*
        debug(DEBUGS, "ANG = %.3f", (float) (clock()-t1)/CLOCKS_PER_SEC);

        fprintf(filetmp, "%.3f ", angle_buffer[0]);
        fprintf(filetmp, "%d %d ", refxyN, actxyN);
        for(i = 0; refxyN > i; i++)
                fprintf(filetmp, "%.3f %.3f ", refxo[i], refyo[i]);
        for(i = 0; actxyN > i; i++)
                fprintf(filetmp, "%.3f %.3f ", actxo[i], actyo[i]);
        fclose(filetmp);

        for(i = 0; number_of_maximums > i; i++) {
                fprintf(filetmp2, "%.3f %.3f ", angle_buffer[i], angle_buffer_correlation[i]);
        }
        fclose(filetmp2);

        debug(DEBUGS, "anglecor.txt written %d", number_of_maximums);*/
        //Sleep(2000);


        /*debug(DEBUGS, "Number of maximums that will be searched: %d, time = %.3f", 
                                   number_of_maximums, (float) (clock()-t1)/CLOCKS_PER_SEC);*/
        
        ////////////////////// Angle calculation end ////////////////////////
        /////////////////////////////////////////////////////////////////////


        t2 = clock();
        /////////////////////////////////////////////////////////////////////
        //////////////////// Translation calculation start //////////////////

        // Go through all potential angles
        for(i = 0; number_of_maximums > i; i++) {

                // Check if the other maximum is good enough 
               // if(angle_buffer_correlation[0] > 2*angle_buffer_correlation[i]) {
                        //debug(DEBUGS, "did this: %.2f > %.2f", angle_buffer_correlation[0], 2*angle_buffer_correlation[i]);
                 //       continue;
                //}

                // The scan arrays are altered so copy fresh ones back on each round
                for(k=0; actxyN >k; k++) {
                        actx[k] = actxo[k];
                        acty[k] = actyo[k];
                }
                for(k=0; refxyN >k; k++) {
                        refx[k] = refxo[k];
                        refy[k] = refyo[k];
                }

                // Rotate act scan with calculated angle
                retpose[0] = 0;
                retpose[1] = 0;
                retpose[2] = -angle_buffer[i];
                rotateAndTranslate(retpose, actx, acty, actxyN);
                
                // Search answer with defined iterating steps
                match_correlation = 0;
                searcher_x_sum = -initialpose[0];
                searcher_y_sum = -initialpose[1];
                t1 = clock();
                for(k = 0; searcher_size > k; k++) {
                        // Calculate how much is refPos - actPos.
                        // Given searcher_x_sum is substracted from actPos coordinates.
                        if(gridMatch(refx, refy, refxyN, 
                                                 actx, acty, actxyN, 
                                                 searcher_x_sum, searcher_y_sum,
                                                 searcher[k], searcher2[k],
                                             &retpose[0], &retpose[1], &bestxy_correlation,
				     //(float)-10, (float)10, (float)0, (float)25,
				     -10, 10,0, 25,
                                                 0, SS) != 0) {
                                                 //k == 1?1:0, SS) != 0) {
                                fprintf(stderr, "mex: error in gridMatch calculation.\n");
                                return -1;
                        }
                        /*
                        debug(DEBUGS, "[angle = %.2f][%d] gridMatch with corr = %.2f dx = %.2f dy = %.2f", 
                                                  -angle_buffer[i]*180/(float)pi, 
                                                  k, bestxy_correlation, retpose[0], retpose[1]);
                        */
                        // Save calculated results
                        match_correlation += bestxy_correlation;
                        searcher_x_sum += retpose[0];
                        searcher_y_sum += retpose[1];
                        

                        // check if anything can come out of this, if not, then stop
                        if(match_correlation >= best_correlation) break;
                }
                
                /*
                debug(DEBUGS, "Time elapsed = %.3f", 
                                       (float) (clock()-t1)/CLOCKS_PER_SEC);
                */
        
                
                /*debug(DEBUGS, "[angle = %.2f][%d] gridMatch with sumcorr = %.2f dx = %.2f dy = %.2f",
                                          -angle_buffer[i]*180/(float)pi, k, match_correlation, searcher_x_sum, searcher_y_sum);
                                          */
                
                // Save best correlation if such was acquired
                if(best_correlation >= match_correlation) {
                        best_correlation = match_correlation;
                        best_x = searcher_x_sum;
                        best_y = searcher_y_sum;
                        best_angle = angle_buffer[i];
                        S[0][0] = SS[0][0];
                        S[0][1] = SS[0][1];
                        S[1][0] = SS[1][0];
                        S[1][1] = SS[1][1];
                }
        }

        /*
        // The scan arrays are altered so copy fresh ones back on each round
        for(k=0; actxyN >k; k++) {
                actx[k] = actxo[k];
                acty[k] = actyo[k];
        }
        for(k=0; refxyN >k; k++) {
                refx[k] = refxo[k];
                refy[k] = refyo[k];
        }

        // Rotate act scan with calculated angle
        retpose[0] = 0;
        retpose[1] = 0;
        retpose[2] = -best_angle;
        rotateAndTranslate(retpose, actx, acty, actxyN);
                
        // Search answer with defined iterating steps
        match_correlation = 0;
        searcher_x_sum = -initialpose[0];
        searcher_y_sum = -initialpose[1];

        for(k = 0; searcher_size > k; k++) {
                // Calculate how much is refPos - actPos.
                // Given searcher_x_sum is substracted from actPos coordinates.
                if(gridMatch(refx, refy, refxyN, 
                                         actx, acty, actxyN, 
                                         searcher_x_sum, searcher_y_sum,
                                         searcher[k], searcher2[k],
                                     &retpose[0], &retpose[1], &bestxy_correlation,
                                         (float)-10, (float)10, (float)0, (float)25,
                                         k == 1?1:0, SS) != 0) {
                        fprintf(stderr, "mex: error in gridMatch calculation.\n");
                        return -1;
                }
                        
                // Save calculated results
                match_correlation += bestxy_correlation;
                searcher_x_sum += retpose[0];
                searcher_y_sum += retpose[1];
                
        }

        // Result was
        best_correlation = match_correlation;
        best_x = searcher_x_sum;
        best_y = searcher_y_sum;
        S[0][0] = SS[0][0];
        S[0][1] = SS[0][1];
        S[1][0] = SS[1][0];
        S[1][1] = SS[1][1];*/
        
        
        //debug(DEBUGS, "LOC = %.3f", (float) (clock()-t2)/CLOCKS_PER_SEC);

        ////////////////////// Translation calculation ends /////////////////
        /////////////////////////////////////////////////////////////////////

        // Return answers
        retCorrection[0] = -best_x;
        retCorrection[1] = -best_y;
        retCorrection[2] = -best_angle;
        *correlation = best_correlation;

        return 0;
}

/**
 * Corrects the angle between two scans by using angles between 
 * scan points. 
 * 
 * @param x1 x-coordinates of first laserscan data.
 * @param y1 y-coordinates of first laserscan data.
 * @param size1 size of x1 and y1 buffers, has to be > step.
 * @param x2 x-coordinates of second laserscan data.
 * @param y2 x-coordinates of second laserscan data.
 * @param size2 size of x2 and y2 buffers, has to be > step.
 * @param searchdist distance to search for answer
 * @param discrete resolution for the search, in radians
 * @param initguess initial estimate for the correction angle
 * @param anglebuffer returned angle corrections in radians.
 * @param corrbuffer correlation sums for returned angle correction in radians.
 *        Gives some hint how believable the returned angle was. If NULL parameter,
 *        then sums are not returned.
 * @param anglebuffersize size of anglebuffer and corrbuffer, this many angles 
 *        is returned at maximum.
 * @param var angle variance.
 *
 * @return number of local angle maximums found, if failed -1
 */
int CAHistogramECorrelationScanMatch::estimateAngle(float *x1, float *y1, int size1, 
                              float *x2, float *y2, int size2, 
                              float searchdist, float discrete, 
                                  float initguess,
                                  float *anglebuffer, float *corrbuffer, int anglebuffersize,
                                  float *var) { 
        //static int searchangleindexes[] = {25,27,28,29,30, 31,32,33,35,37, 39,40,41,42,43}; // Search indexes for small angle algorithm
        //static int searchangleindexes[] = {27, 32, 35, 38}; // Search indexes for small angle algorithm
			  ///Tune These to find better lines!!
        //static int searchangleindexes[] = {27, 35, 41}; // Search indexes for small angle algorithm
				static int searchangleindexes[] = {10, 15, 20,27}; // Search indexes for small angle algorithm
        static int searchangleindexes_index = 4;        // Size of search index array, linear to processing time
        static int number_of_angles_per_search = 4;     // Number of local maximums
        static float buffer[2][3*4];                     // Buffer for angles and correlations
        int number_of_maximums = 0;                                             // Number of maximums found

        float meanangle = 0;
        float var_mean = 0;                                                             // var[searchangleindexes_index] average
        
        int i = 0, tmp = 0, tmpsum = 0;

        // Check parameters
        if(x1==NULL || y1 == NULL || size1 <= 0 ||
           x2==NULL || y2 == NULL || size2 <= 0 || 
           anglebuffer == NULL || corrbuffer == NULL ||
           anglebuffersize <= 0) {
                fprintf(stderr, "estimateAngle: invalid paremeters.");
                return -1;
        }else if(searchangleindexes[0] + 5 > size1 || searchangleindexes[0] + 5 > size2) {
                // We want to line at least 5 lines
                fprintf(stderr, "estimateAngle: too small number of scan points(%d).", size1);
                return -1;
        }

        // Calculate angle correction using small lines
        for(i = 0; searchangleindexes_index > i; i++) {
                // Search with this index
                if((tmp = smallAngleCorrection(x1, y1, size1, 
                                                                           x2, y2, size2,
                                                                           searchdist, discrete, // search area, resolution
                                                                           initguess, // initial "guess"
                                                                   searchangleindexes[i], 
                                                                       &buffer[0][tmpsum], &buffer[1][tmpsum], 
                                                                           number_of_angles_per_search,
                                                                           var, &meanangle)) <= 0) {
                        fprintf(stderr, "estimateAngle: smallangleCorrelation failed.");
                        return -1;
                }
                var_mean += *var;
                // increase the counter
                tmpsum += tmp;
        }
        var_mean /= searchangleindexes_index;

        // Estimate angle based on values saved in the buffer
        number_of_maximums = arrayAngleEstimation(buffer[0], buffer[1], tmpsum, &meanangle, var, (float)(1.0*pi/180.0));

#ifdef ENABLE_CORRELATION_DEBUG
        /*
        debug(DEBUGS, "Found %d angles [angle, correlation] (listed below):", number_of_maximums);
        for(i = 0; number_of_maximums > i; i++) 
                debug(DEBUGS, "%.4f %.4f", buffer[0][i]*180/pi, buffer[1][i]);
        */
#endif
        
        // Copy data for return
        for(i = 0; number_of_maximums > i && anglebuffersize > i; i++)  {
                anglebuffer[i] = buffer[0][i];
                corrbuffer[i]  = buffer[1][i];
        }
        *var = var_mean;

        return i;
}

/**
 * Compare int arrays. Arrays are compared using x values,
 * smallest value is on the first place of returned array.
 * 
 * @a element1 to compare
 * @b element2 to compare
 * @return < 0 if a is before b, > 0 if b is before a.
 */
static int compareIntpointXAscending(const void * a, const void * b) { 
        return ((intpoint_t*)a)->y - ((intpoint_t*)b)->y;
}

/**
 * Compare int arrays. Arrays are compared using x values,
 * biggest value is on the first place of returned array.
 * 
 * @a element1 to compare
 * @b element2 to compare
 * @return < 0 if a is before b, > 0 if b is before a.
 */
static int compareIntpointXDescending(const void * a, const void * b) { 
        return -(((intpoint_t*)a)->x - ((intpoint_t*)b)->x);
}

/**
 * Compare function for two angleAndCorrection_t, used for quick sort.
 * 
 * @a element1 to compare
 * @b element2 to compare
 * @return < 0 if a is before b, > 0 if b is before a.
 */
static int compareangleAndCorrelation(const void * a, const void * b) { 
        if(((angleAndCorrelation_t*)a)->correlation > ((angleAndCorrelation_t*)b)->correlation) {
                return -1;
        }else if(((angleAndCorrelation_t*)a)->correlation == ((angleAndCorrelation_t*)b)->correlation)
 {
                return 0;
        }       
        return 1;
}


/**
 * Corrects the angle between two scans by using angles between 
 * scan points. Used points are separated with step number of points.
 * Correction is calculated using "histogram correlation matching".
 * 
 * @param x1 x-coordinates of first laserscan data.
 * @param y1 y-coordinates of first laserscan data.
 * @param size1 size of x1 and y1 buffers, has to be > step.
 * @param x2 x-coordinates of second laserscan data.
 * @param y2 x-coordinates of second laserscan data.
 * @param size2 size of x2 and y2 buffers, has to be > step.
 * @param searchdist distance to search for answer
 * @param discrete resolution for the search, in radians
 * @param initguess initial estimate for the correction angle
 * @param step number of points within one angle approximation
 * @param anglebuffer returned angle corrections in radians.
 * @param corrbuffer correlation sums for returned angle correction in radians.
 *                Gives some hint how believable the returned angle was. If NULL parameter,
 *        then sums are not returned.
 * @param anglebuffersize size of anglebuffer and corrbuffer, this many angles 
 *        is tried to be returned.
 * @param var variance to be returned.
 * @param mean angle mean value to be returned.
 *
 * @return number of local angle maximums found, if failed -1
 */
int CAHistogramECorrelationScanMatch::smallAngleCorrection(float *x1, float *y1, int size1, 
                                     float *x2, float *y2, int size2, 
                                     float searchdist, float discrete, 
                                                 float initguess, 
                                                 int step, 
                                                 float *anglebuffer, float *corrbuffer, int anglebuffersize,
                                                 float *var, float *mean) {
        static int kk[SMALL_ANGLE_DISCRETE_ANGLES_MAX];         // discereted angles from scan1 will be saved here.
        static int kk2[SMALL_ANGLE_DISCRETE_ANGLES_MAX];        // discereted angles from scan2 will be saved here.
        int sizek          = (int)ceil(pi/discrete) + 1; // Discreted angles go 0:discrete:pi, size of kk and kk2 arrays.
        int sarea      = (int)(searchdist/discrete); // This many steps are searched to both directions.
        int initoffset = (int)(initguess/discrete);  // Initguess moves the search area center with this.
        int sum  = 0, summax = 0;               // Correlation sum and maximum correlation sum.
        int sumi = 0;                                   // index of maximum correlation sum.
        static intpoint_t sumarray[SMALL_ANGLE_SUMARRAYINDEX_MAX]; // sums are saved here and maximums are foud among these. 
        static int corrsumarray[SMALL_ANGLE_DISCRETE_ANGLES_MAX];  // Correlation sums are saved here
        int sumarrayindex = 1;                  // Points to place in sumarray where next sum is saved
        int sumprev = 0, sumprev2 = 0;  // Previous sums are saved here so that we can determine if we found 
                                                                        // local maximum for correlation or not.
        int sumcorr = 0, meanangle = 0; // Sum of all angle correlations and mean angle

        // temporary and multipurpose variables
        int i = 0, k = 0, j = 0;
        int index = 0;   

#ifdef ENABLE_CORRELATION_DEBUG
        // This is file where sumarray is saved for debug
        FILE *file = fopen("smallangle_correlation_sums.txt", "w");
#endif

        // Check parameters
        if(x1==NULL || y1 == NULL || size1 < step || anglebuffersize <= 0 ||
           x2==NULL || y2 == NULL || size2 < step || anglebuffer  == NULL ||
           sarea < 0 || 2*sarea + 1 > SMALL_ANGLE_DISCRETE_ANGLES_MAX || 
           sizek > SMALL_ANGLE_DISCRETE_ANGLES_MAX) {
                fprintf(stderr, "smallAngleCorrection: invalid paremeters.");
                return -1;
        }

        // Set initial values to zero
        for(i = 0; sizek > i; i++) {
                kk[i] = 0; kk2[i] = 0;
        }

        // Calculate discrete values for angles, the slope
        for(i = 0; size1 - step > i; i++)
                kk[(int)floor((atan((double)(y1[i+step]-y1[i])/(double)(x1[i+step]-x1[i]))+pi/2)/discrete)]  += 1; 
        for(i = 0; size2 - step > i; i++)
                kk2[(int)floor((atan((double)(y2[i+step]-y2[i])/(double)(x2[i+step]-x2[i]))+pi/2)/discrete)] += 1; 

        // Search given area for maximums
        for(i = -sarea+initoffset; sarea+initoffset > i; i++) {
                sum = 0;
                // With correction-shift i, calculate the correlation sum
                index = (i+1)%sizek; // first shift
                if(index < 0) index += sizek; // Matlab modulo does this automagically
                for(k = 1; sizek > k; k++) {
                        sum += kk[k] * kk2[index];
                        // Increase index by one
                        index++;
                        if(index >= sizek) index = 0;
                }

                // Save some values for mean and variance calculation
                corrsumarray[j] = sum;
                j++;
                sumcorr   += sum;
                meanangle += i * sum;

                // if space for maximums left
                if(sumarrayindex < SMALL_ANGLE_SUMARRAYINDEX_MAX) {
                        // find if prev was local maximum
                        if(sumprev > sumprev2 && sumprev > sum) {
                                sumarray[sumarrayindex].x = sumprev;
                                sumarray[sumarrayindex].y = i-1;
                                sumarrayindex++;
                        }
                        sumprev2 = sumprev;
                        sumprev  = sum;
                }else {
                        fprintf(stderr, "smallAngleCorrection: not able to save all maximums!");
                }

#ifdef ENABLE_CORRELATION_DEBUG
                // Print the sum
                fprintf(file, "%.6f %d\n", (float)(i*discrete/pi*180), sum);
#endif
                // Was maximum found, biggest maximum is always in sumarray[0]
                if(summax < sum) {
                        sumarray[0].x = sum;
                        sumarray[0].y = i;
                        summax = sum;
                        sumi   = i;
                }
        }

        // Calculate mean and variance for calculated angles
        *mean = (float)((float)meanangle)/((float)sumcorr);
        j = 0;
        for(i = -sarea+initoffset; sarea+initoffset > i; i++) {
                *var = (i - *mean)*(i - *mean)*corrsumarray[j];
                j++;
        }
        *var = *var/sumcorr*discrete*discrete;

        // Sort results according to sums. Angle with biggest correlation
        // sum is in place 0, second in place 1 and etc.
        qsort(sumarray, sumarrayindex, sizeof(intpoint_t), compareIntpointXDescending);
	// (int(*)(const void*, const void*))
        
        // Remove duplicates and save best correlated angles for return.
        k = 0;
        if(sumarrayindex > 1) {
                if(sumarray[0].y == sumarray[1].y) k = 1;
        }
        // Save answers, angle between scans and correlation sums
        for(i = k; sumarrayindex > i && anglebuffersize > i-k; i++) {
                anglebuffer[i-k] = (float)sumarray[i].y * discrete;
                if(corrbuffer != NULL)
                        corrbuffer[i-k] = (float)sumarray[i].x;
        }

#ifdef ENABLE_CORRELATION_DEBUG 
        fclose(file);
#endif

        return i-k;
}

/**
 * Corrects translation error with "grid matching". Given scan coordinates
 * are discreted in the area defined by min and max. Then coordinates
 * will be sorted according to x-value using qsort. Then for each point in
 * first scan, a nearest point from second scan is searched. The nearest point 
 * is estimated to be hit(== 0) or miss(==1), or something between those two.
 * The better the scan correlation is, the smaller the correlation sum is.
 * 
 * @param x1 x-coordinates of first data.
 * @param y1 y-coordinates of first data.
 * @param size1 size of x1 and y1 buffers, has to be > 0 but <= 361.
 * @param x2 x-coordinates of second data.
 * @param y2 x-coordinates of second data.
 * @param size2 size of x2 and y2 buffers, has to be > 0 but <= 361.
 * @param deltax2 change to be subtracted from x2 coordinates.
 * @param deltay2 change to be subtracted from y2 coordinates.
 * @param searchdist distance to search for answer.
 * @param stepsize resolution for the search.
 * @param dx correction in x-direction.
 * @param dy correction in y-direction.
 * @param best_correlation correlation with dx and dy.
 * @param minx minimum x-coordinate for search.
 * @param maxx maximum x-coordinate for search.
 * @param miny minimum y-coordinate for search.
 * @param maxy maximum y-coordinate for search.
 * @param get_probability_mask 1 if mask is wanted, else 0.
 * @param S covariance matrix, size 2x2
 * @return 0 if all ok, else -1
 */
int CAHistogramECorrelationScanMatch::gridMatch(float *x1, float *y1, int size1, 
                          float *x2, float *y2, int size2, 
                          float deltax2, float deltay2,
                          float searchdist, float stepsize,
                          float *dx, float *dy, float *best_correlation,
                          int minx, int maxx, int miny, int maxy,
                          int get_probability_mask, float **S) {
       int searchI = 0; // this many indexes has to be searched in every direction
        static intpoint_t indexes1[361], indexes2[361]; // Indexes calculated from coordinates
        int indexes1_size = 0, indexes2_size = 0;       // Number of valid indexes
        int best_x = 0, best_y = 0;           // Smallest number of x and y coordinates
        int res = 2;//, min_res = 0;                      // Resolution to search from
        
        float corr = 0;                                           // Correlation of points and smallest distance resolition
        int ret = 0;

        static float maskdata[15][15];
        static float *mask[15];
        static int mask_size = 15;
        
        int x = 0, y = 0, i = 0;//, r = 0, r_new = 0;
//      clock_t t1;

#ifdef ENABLE_CORRELATION_DEBUG
        FILE *place = fopen("place", "w");
#endif

        // Check parameters
        if(x1==NULL || y1 == NULL || size1 <= 0 || size1 > 361 ||
           x2==NULL || y2 == NULL || size2 <= 0 || size2 > 361 ||
           dx == NULL || dy == NULL || best_correlation == NULL ||
           S == NULL) {
                fprintf(stderr, "gridMatch: invalid parameter(s).");
                return -1;
        }

        // Create probability mask
        for(i = 0; mask_size > i; i++) {
                mask[i] = maskdata[i];
        }

        // Limits for search and malloc correlation matrix
        searchI = (int)ceil(searchdist/stepsize);
        *best_correlation = 666;

        // Select points that belong to between limits and calculate
        // indexes in a "search matrix" when transleted with minx and miny.
        for(i = 0; size1 > i; i++) {
                if(x1[i] > minx && x1[i] < maxx && y1[i] > miny && y1[i] < maxy) {
                        indexes1[indexes1_size].x = (int)floor((x1[i]-minx)/stepsize + 0.5);
                        indexes1[indexes1_size].y = (int)floor((y1[i]-miny)/stepsize + 0.5);
                        indexes1_size++;
                }
        }
        for(i = 0; size2 > i; i++) {
                if(x2[i]-deltax2 > minx && x2[i]-deltax2 < maxx && y2[i]-deltay2 > miny && y2[i]-deltay2 < maxy) {
                        // round(X) = floor(X+0.5), stupid c
                        indexes2[indexes2_size].x = (int)floor((x2[i]-deltax2-minx)/stepsize + 0.5);
                        indexes2[indexes2_size].y = (int)floor((y2[i]-deltay2-miny)/stepsize + 0.5);
                        indexes2_size++;
                }
        }

        // Sort indexes so that they can be compared more easily
        qsort(indexes1, indexes1_size, sizeof(intpoint_t), (int(*)(const void*, const void*))compareIntpointXAscending);
        qsort(indexes2, indexes2_size, sizeof(intpoint_t), (int(*)(const void*, const void*))compareIntpointXAscending);

        // Calculate spatial correction by shifting x and y
        for(y = -searchI; searchI >=y; y++) {
                for(x = -searchI; searchI >=x; x++) {
                        corr = 0;
                        //t1 = clock();

                        //for(j = 0; 1000 > j; j++) {
                        // Calculate closest point for each point
                        //      corr = 0;
                        findClosestPoint(indexes1, indexes1_size,
                                                         indexes2, indexes2_size,
                                                         x, y, res, get_probability_mask, 
                                                         best_correlation, &corr);
                        //}
                                                         
                        //debug(DEBUGS, "STime elapsed = %.3f", (float)(clock()-t1)/CLOCKS_PER_SEC);

                        // Save probabilty mask if needed
                        if(get_probability_mask) {
                                if(y+searchI < mask_size && x+searchI < mask_size)
                                        mask[y+searchI][x+searchI] = corr;
                        }

                        // Save new best match if such occured
                        if(*best_correlation > corr) {
                                *best_correlation = corr;
                                best_x = x;
                                best_y = y;
                        }
#ifdef ENABLE_CORRELATION_DEBUG
                        fprintf(place, "%d %d %.2f ", x, y, corr);
#endif

                }       

#ifdef ENABLE_CORRELATION_DEBUG
                fprintf(place, "\n", x, y, corr);
#endif

        }

#ifdef ENABLE_CORRELATION_DEBUG
        fclose(place);
#endif

        // Approximate the probability with ellipse
        if(get_probability_mask) {
                ret = paraboloidEstimation(mask, mask_size > 2*searchI+1?2*searchI+1:mask_size, S);
                //debug(DEBUGS, "para ret = %d", ret);
                //debug(DEBUGS, "varx = %.3f vary = %.3f covxy = %.3f", S[0][0], S[1][1], S[0][1]);
                //Sleep(1000);
                if(ret == 0) { //&& (S[0][0] < 10 && S[1][1] < 10)) {
                        S[0][0] *= stepsize*stepsize;
                        S[1][1] *= stepsize*stepsize;   
                        S[0][1] *= stepsize*stepsize;
                        S[1][0] *= stepsize*stepsize;   
                        
                        if(sqrt(S[0][0])*2.0 > searchdist*2.0+stepsize ||
                                sqrt(S[1][1])*2.0 > searchdist*2.0+stepsize) {
                                //debug(DEBUGS, "S0 %.3f S1 %.3f d %.3f", S[0][0], S[1][1], (searchdist*2.0+stepsize)*(searchdist*2.0+stepsize)/4.0);
                                float maxxx = S[0][0] > S[1][1] ? S[0][0]:S[1][1];
                                double kk = ((((searchdist*2.0+stepsize)*(searchdist*2.0+stepsize))/4.0)/maxxx); 
                                //debug(DEBUGS, "kk = %.6lf", kk);
                                //debug(DEBUGS, "%.3f %.3f %.3f", maxxx, S[0][0], S[1][1]);
                                S[0][0] *= kk;
                                S[1][1] *= kk;  
                                S[0][1] *= kk;
                                S[1][0] *= kk;  
                        }
                }else {
                        S[0][0] = (searchdist*2.0+stepsize)*(searchdist*2.0+stepsize)/4.0;
                        S[1][1] = (searchdist*2.0+stepsize)*(searchdist*2.0+stepsize)/4.0;
                        S[0][1] = 0; //searchdist*searchdist;
                        S[1][0] = 0; //searchdist*searchdist;
                }
                //ellipseEstimation(mask, *mask_size > 2*searchI+1?2*searchI+1:*mask_size,
                //      *mask_size > 2*searchI+1?2*searchI+1:*mask_size);
        }

        //debug(DEBUGS, "STime elapsed = %.3f", (float)(clock()-t1)/CLOCKS_PER_SEC);

        // save the return values and return
        *dx = best_x*stepsize;
        *dy = best_y*stepsize;
        *best_correlation = indexes2_size == 0?300:*best_correlation/indexes2_size*300;

        return 0;
}

int CAHistogramECorrelationScanMatch::findClosestPoint(intpoint_t *indexes1, int indexes1_size,
                                         intpoint_t *indexes2, int indexes2_size,
                                         int x, int y, int res,
                                         int get_probability_mask, 
                                         float *best_correlation, float *corr) {
        int deltax = 0, deltaxy = 0;    // distance in y direction and in xy direction
        int r, r_new = 0;
        int min_res;
        int i = 0;
                        
        // For each point in indexes2, find closest indexes1
        for(i = 0; indexes2_size > i; i++) {
                // is set of for each index
                min_res = 2*res+1;
                r = r_new;

                // Go to right place in the indexes
                while(indexes1[r].y + y + res < indexes2[i].y ) {
                        r++;
                        if(r >= indexes1_size) break;
                }
                r_new = r;
                // No suitable indexes1 was found
                if(r >= indexes1_size) {
                        //corr += 1;
                        *corr += indexes2_size-i;
                        break;
                }

                // Go through potential indexes to find smallest distance
                while(indexes1[r].y + y - res <= indexes2[i].y) {
                        deltax = abs(indexes1[r].x + x - indexes2[i].x);
                        if(deltax <= res) {
                                deltaxy = abs((indexes1[r].y + y - indexes2[i].y)) + deltax;
                                if(deltaxy < min_res) min_res = deltaxy;
                                if(deltaxy == 0) break; //can't be smaller
                        }
                        r++;
                        if(r >= indexes1_size) break;
                }

                // Update correlation
                *corr += (float)min_res/(float)(2*res+1);

                // If correlation is already so big that it can't save the situation, give up
                if(!get_probability_mask && *corr >= *best_correlation) break;
        }

        return 0;
}

/**
 * Estimates the given probability mask with ellipse.
 *
 * @param mask place probability mask, small value is more probable.
 * @param msize width and height of the mask.
 * @param S covariance matrix, size is 2x2
 * @return 0 if estimation valid, 1 if not valid, -1 if failed
 */
int CAHistogramECorrelationScanMatch::paraboloidEstimation(float **mask, int msize, float **S) {
        static float max = 361; // All mask values should be less than this
        float param_est[6];     // Parameters estimations matrix
        float sum = 0;          // Normalizing sum.
        float det = 0;                  // Determinant
        int i = 0, k = 0;

        // Check parameters
        if(mask == NULL || msize <= 0 || S == NULL) {
                fprintf(stderr, "ellipseEstimation: invalid parameters given.");
                return -1;
        }

        // Calculate sum for normalizing
        for(i = 0; msize > i; i++) {
                for(k = 0; msize > k; k++) {
                        sum += mask[i][k];
                }
        }
        sum = max*msize*msize - sum;
        
        // Normalize mask
        for(i = 0; msize > i; i++) {
                for(k = 0; msize > k; k++) {
                        det = (float)(max - mask[i][k])/(float)sum;
                        mask[i][k] = -2*log(det);
                }
        }

        // Multiply with inv(H'*H)*H'
        if(multiplyHx(mask, msize, param_est) != 0)
                return -1;
        //debug(DEBUGS, "%.3f r1 = %.3f r3 = %.3f r2 = %.3f", sum, param_est[0], param_est[1], param_est[2]);

        // Parse covariance matrix
        det = param_est[0]*param_est[1]-param_est[2]*param_est[2]/4;
        if(det <= 0) return 1;
        S[0][0] = param_est[1]/det;
        S[1][1] = param_est[0]/det;
        S[1][0] = -param_est[2]/2/det;
        S[0][1] = S[1][0];
        if(S[0][0] <= 0 || S[1][1] <= 0)
                return 1;

        return 0;
}


/**
 * Rotates and translates the given x,y-points with pos. The angle correction
 * is done first, then the required translation.
 *
 * @param pos translation x = pos(1), y = pos(2). Rotation angle = pos(3).
 * @param xpoints x-points of scan
 * @param ypoints y-points of scan
 * @return 0 if all ok, else failed
 */
int CAHistogramECorrelationScanMatch::rotateAndTranslate(float pos[3], float *xpoints, 
						 float *ypoints, int points_size) {
        float newx = 0, newy = 0; // temp values for new cordinates
        int i = 0;

        // Check parameters
        if(points_size <= 0 || xpoints == NULL || ypoints == NULL) {
                fprintf(stderr, "rotateAndTranslate: invalid parameters.\n");
                return -1;
        }
                
        // Rotation 
        for(i = 0; points_size > i; i++) {
                newx = (float)(cos(pos[2])*xpoints[i] - sin(pos[2])*ypoints[i]);
                newy = (float)(sin(pos[2])*xpoints[i] + cos(pos[2])*ypoints[i]);
                // Translation
                xpoints[i] = newx + pos[0];
                ypoints[i] = newy + pos[1]; 
        }

        return 0;
}

/**
 * Sorts the given angle estimates and removes duplicated 
 * angles. Number of found suitable angles is returned.
 * 
 * @param anglebuffer array containing angles, returned angles 
 *        are also save in here.
 * @param correlation values are saved here
 * @param size size of anglebuffer array.
 * @param meanangle mean angle value
 * @param var variance for angle
 * @param resolution defines the distance within which angles are considered as one
 * @return number of angles, -1 if failed
 */
int CAHistogramECorrelationScanMatch::arrayAngleEstimation(float *anglebuffer, float *corbuffer, int size,
                                                 float *meanangle, float *var, float resolution) {
        float prev = 0;   // Previous angle is saved here for comparison purposes
        static angleAndCorrelation_t tmp[150];
        float anglecortemp[150];
        //float sumtmp = 0;
        int i = 0, k = 0, j = 0; // Temporary multipurpose variable

        // Check given parameters
        if(anglebuffer == NULL || size <= 0) {
                fprintf(stderr, "arrayAngleEstimation: invalid paremeters.");
                return -1;
        }

        // Only if the 
        if(size > 150) {
                size = 150;
                fprintf(stderr, "The number of angles too big. Truncated to 150");
        }

        for(i = 0; size > i; i++) {
                // Algorithm sorts according correlation, and we want to sort angles :=)
                tmp[i].angle         = corbuffer[i];
                tmp[i].correlation   = anglebuffer[i];
        }

        // sort angles and remove duplicates
        qsort(tmp, size, sizeof(angleAndCorrelation_t), compareangleAndCorrelation);
        prev = (float)66666666;
        k = 0;
        for(i = 0; size > i; i++) {
                // duplicate?

               if(fabs(tmp[i].correlation - prev) < resolution/2 ||
                   fabs(prev - tmp[i].correlation) < resolution/2 ) {
                        tmp[k-1].correlation = (tmp[k-1].correlation+tmp[i].correlation)/2;
                        tmp[k-1].angle += tmp[i].angle; // remember, we sum correlation
                        anglecortemp[k-1] += tmp[i].angle;
                }else {
                        //tmp[i].correlation != prev) {
                        tmp[k] = tmp[i];
                        anglecortemp[k] = tmp[i].angle;
                        k++;
                }

                prev = tmp[k-1].correlation;
        }

        // Add correlations which are near to the angle value
        for(i = 0; k > i; i++) {
                for(j = 0; k > j; j++) {
                        if(j == i) continue;
                        // save only non duplicate values
                        if(fabs(tmp[i].correlation - tmp[j].correlation) < resolution ||
                           fabs(tmp[j].correlation - tmp[i].correlation) < resolution ) {
                                tmp[i].angle += anglecortemp[j];
                        }
                }

        }

        // We have #k different angles, sort according to correlation
        for(i = 0; k > i; i++) {
                prev = tmp[i].angle;
                tmp[i].angle = tmp[i].correlation;
                tmp[i].correlation = prev;
        }
        qsort(tmp, k, sizeof(angleAndCorrelation_t), compareangleAndCorrelation);
        
        // Save results
        for(i = 0; k > i; i++) {
                anglebuffer[i] = tmp[i].angle;
                corbuffer[i]   = tmp[i].correlation;
        }

        return k;
}



/**
 * Multiply probability matrix with right size position matrix.
 *
 * @param probability normalized matrix with probabilities. Size is psize x psize.
 * @param psize probability matrix size.
 * @param est parameter estimations returned.
 * @return 0 if all ok, else -1.
 */
int CAHistogramECorrelationScanMatch::multiplyHx(float **probability, int psize, float *est) {
	static const float H2x2[6][25] =
	  { {(float)0.028571, (float)0.028571, (float)0.028571, (float)0.028571, (float)0.028571, (float)-0.014286, (float)-0.014286, (float)-0.014286, (float)-0.014286, (float)-0.014286, (float)-0.028571, (float)-0.028571, (float)-0.028571, (float)-0.028571, (float)-0.028571, (float)-0.014286, (float)-0.014286, (float)-0.014286, (float)-0.014286, (float)-0.014286, (float)0.028571, (float)0.028571, (float)0.028571, (float)0.028571, (float)0.028571, },
	    { (float)0.028571, (float)-0.014286, (float)-0.028571, (float)-0.014286, (float)0.028571, (float)0.028571, (float)-0.014286, (float)-0.028571, (float)-0.014286, (float)0.028571, (float)0.028571, (float)-0.014286, (float)-0.028571, (float)-0.014286, (float)0.028571, (float)0.028571, (float)-0.014286, (float)-0.028571, (float)-0.014286, (float)0.028571, (float)0.028571, (float)-0.014286, (float)-0.028571, (float)-0.014286, (float)0.028571, },
	    { (float)0.040000, (float)0.020000, (float)0.000000, (float)-0.020000, (float)-0.040000, (float)0.020000, (float)0.010000, (float)0.000000, (float)-0.010000, (float)-0.020000, (float)0.000000, (float)0.000000, (float)0.000000, (float)0.000000, (float)0.000000, (float)-0.020000, (float)-0.010000, (float)0.000000, (float)0.010000, (float)0.020000, (float)-0.040000, (float)-0.020000, (float)0.000000, (float)0.020000, (float)0.040000, },
	    { (float)-0.040000, (float)-0.040000, (float)-0.040000, (float)-0.040000, (float)-0.040000, (float)-0.020000, (float)-0.020000, (float)-0.020000, (float)-0.020000, (float)-0.020000, (float)0.000000, (float)0.000000, (float)0.000000, (float)0.000000, (float)0.000000, (float)0.020000, (float)0.020000, (float)0.020000, (float)0.020000, (float)0.020000, (float)0.040000, (float)0.040000, (float)0.040000, (float)0.040000, (float)0.040000, },
	    { (float)-0.040000, (float)-0.020000, (float)0.000000, (float)0.020000, (float)0.040000, (float)-0.040000, (float)-0.020000, (float)0.000000, (float)0.020000, (float)0.040000, (float)-0.040000, (float)-0.020000, (float)0.000000, (float)0.020000, (float)0.040000, (float)-0.040000, (float)-0.020000, (float)0.000000, (float)0.020000, (float)0.040000, (float)-0.040000, (float)-0.020000, (float)0.000000, (float)0.020000, (float)0.040000, },
	    { (float)-0.074286, (float)0.011429, (float)0.040000, (float)0.011429, (float)-0.074286, (float)0.011429, (float)0.097143, (float)0.125714, (float)0.097143, (float)0.011429, (float)0.040000, (float)0.125714, (float)0.154286, (float)0.125714, (float)0.040000, (float)0.011429, (float)0.097143, (float)0.125714, (float)0.097143, (float)0.011429, (float)-0.074286, (float)0.011429, (float)0.040000, (float)0.011429, (float)-0.074286, }
	};
	int i = 0, j = 0, k = 0;

	// Check parameters
	if(probability == NULL || (psize != 5 && psize != 5) || est == NULL) {
		fprintf(stderr, "ellipseEstimation: invalid parameters given.");
		return -1;
	}

	// Calculate paramter fit estimations
	if(psize == 5) {
		for(k = 0; 6 > k; k++) {
			est[k] = 0;
		}
		for(i = 0; psize > i; i++) {
			for(j = 0; psize > j; j++) {
				for(k = 0; 6 > k; k++) {
					est[k] += H2x2[k][5*i+j]*probability[j][i];
				}
			}
		}
	}

	return 0;
}

/**
 * Converts given distance to x-y coordinates
 *
 * @param R distance array, size is 361, if distance < 0 then
 * it is ignored.
 * @param x x-coordinate
 * @param y y-coordinate
 * @return number of coordinates, or -1 if fails
 */
int CAHistogramECorrelationScanMatch::distance2xycoordinates(float *R, float *x, float *y) {
        int i = 0, ret = 0;

        // Check parameters
        if(R == NULL || x == NULL || y == NULL) {
                fprintf(stderr, "distance2xycoordinates: invalid parameters.");
                return -1;
        }

        // convert to xy points
        for(i = 0; 361 > i; i++) {
                if(R[i] >= 0) {
                        x[ret] = (float)(cos(i*0.5/180.0*pi)*R[i]);
                        y[ret] = (float)(sin(i*0.5/180.0*pi)*R[i]);
                        ret++;
                }
        }

        return ret;
}

/**
 * Converts given distance to x-y coordinates
 * Improved algorithm. Uses the new interface and is able to deal with arbitrary 
 * sized scans 
 * @param ScanMatch::scan scan, if distance < 0 then
 * it is ignored.
 * @param x x-coordinate
 * @param y y-coordinate
 * @return number of coordinates, or -1 if fails
 */
int CAHistogramECorrelationScanMatch::distance2xycoordinates(ScanMatch::scan &m, float *x, float *y) {
		int i = 0, ret = 0;

        // Check parameters
		if(m.r == NULL || x == NULL || y == NULL) {
				fprintf(stderr, "distance2xycoordinates: invalid parameters.");
				return -1;
		}

        // convert to xy points
		for(i = 0; m.N > i; i++) {
				if(m.r[i] >= 0) {
						//x[ret] = (float)(cos(i*180.0*pi)*m.r[i]);
						//y[ret] = (float)(sin(i*180.0*pi)*m.r[i]);
						x[ret] = (float)(cos(m.a[i]-m.a[0])*m.r[i]);
						y[ret] = (float)(sin(m.a[i]-m.a[0])*m.r[i]);
						ret++;
				}
		}

		return ret;
}


/**
 * Integrates the given differential Pose into original
 * pose. 
 * 
 * @param original Original Pose, size is 3
 * @param differential Differential change which is to be 
 *        integrated to the pose. Size is also 3(x, y, angle).
 * @param result Integrated pose is saved here.
 * @return 0 if all ok, else -1
 */
int CAHistogramECorrelationScanMatch::integrate(float *original, float *differential, float *result) {
        // Check parameters
        if(differential == NULL || original == NULL || 
           result == NULL || differential == result) {
                fprintf(stderr, "integrate: invalid parameters.");
                return -1;
        }

        // x and y direction, rotate the distances to the direction of original[2]
        result[0] = original[0] + (float)cos(original[2])*differential[0] - (float)sin(original[2])*differential[1];
        result[1] = original[1] + (float)sin(original[2])*differential[0] + (float)cos(original[2])*differential[1];

        // Update angle
        result[2] = (float)fmod(differential[2] + original[2], 2*pi);
        if(result[2] < 0) result[2] += 2*(float)pi; // fmod accepts also negative values

        return 0;
}


